/**
 * @file ebex_genetic_seed.c
 *
 * @date Oct 24, 2011
 * @author seth
 *
 * @brief This file is part of EBEX State Estimator, created for the EBEX project
 *
 * This software is copyright (C) 2011 Columbia University
 *
 * EBEX State Estimator is free software; you can redistribute it and/or modify
 * it under the terms of the GNU General Public License as published by
 * the Free Software Foundation; either version 2 of the License, or
 * (at your option) any later version.
 *
 * EBEX State Estimator is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 * GNU General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License
 * along with EBEX State Estimator; if not, write to the Free Software Foundation, Inc.,
 * 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
 *
 */
#include <stdbool.h>
#include <stddef.h>

#include <ebex_genetic.h>

bool ebex_genetic_seed_dbl_rnd_uniform(population_t *m_pop, organism_t *m_seed, size_t m_chromosome, size_t m_allele)
{
	/* Checks. */
	if (!m_pop)
		die("Null pointer to population structure passed.");
	if (!m_seed)
		die("Null pointer to entity structure passed.");

	if (m_seed->chromosome[m_chromosome].rnd.type != random_dist_none)
	{
		/* Seeding. */
		m_seed->chromosome[m_chromosome].dbl_allele[m_allele] = ran_double_range_uniform(m_seed->chromosome[m_chromosome].allele_min_double,
				m_seed->chromosome[m_chromosome].allele_max_double);
	}

	return true;
}

bool ebex_genetic_seed_dbl_rnd_unit_gaussian(population_t *m_pop, organism_t *m_seed, size_t m_chromosome, size_t m_allele)
{

	/* Checks. */
	if (!m_pop)
		die("Null pointer to population structure passed.");
	if (!m_seed)
		die("Null pointer to entity structure passed.");

	if (m_seed->chromosome[m_chromosome].rnd.type != random_dist_none)
	{
		/* Seeding. */
		m_seed->chromosome[m_chromosome].dbl_allele[m_allele] += ran_gaussian(0.0, 1.0);
		m_seed->chromosome[m_chromosome].dbl_allele[m_allele] = CLAMP(m_seed->chromosome[m_chromosome].dbl_allele[m_allele],
				m_seed->chromosome[m_chromosome].allele_min_double, m_seed->chromosome[m_chromosome].allele_max_double);
	}

	return true;
}

bool ebex_genetic_seed_dbl_rnd_sample(population_t *m_pop, organism_t *m_seed, size_t m_chromosome, size_t m_allele)
{
	/* Checks. */
	if (!m_pop)
		die("Null pointer to population structure passed.");
	if (!m_seed)
		die("Null pointer to entity structure passed.");

	/* Seeding. */
	m_seed->chromosome[m_chromosome].dbl_allele[m_allele] += ran_sample(&m_seed->chromosome[m_chromosome].rnd);
	m_seed->chromosome[m_chromosome].dbl_allele[m_allele] = CLAMP(m_seed->chromosome[m_chromosome].dbl_allele[m_allele],
			m_seed->chromosome[m_chromosome].allele_min_double, m_seed->chromosome[m_chromosome].allele_max_double);

	return true;
}

bool ebex_genetic_seed_dbl_zero(population_t *m_pop, organism_t *m_seed, size_t m_chromosome, size_t m_allele)
{
	/* Checks. */
	if (!m_pop)
		die("Null pointer to population structure passed.");
	if (!m_seed)
		die("Null pointer to entity structure passed.");

	if (m_seed->chromosome[m_chromosome].rnd.type != random_dist_none)
	{
		/* Seeding. */
		m_seed->chromosome[m_chromosome].dbl_allele[m_allele] = 0.0;
	}

	return true;
}

bool ebex_genetic_seed_quat_random(population_t *m_pop, organism_t *m_seed, size_t m_chromosome, size_t m_allele)
{

	/* Checks. */
	if (!m_pop)
		die("Null pointer to population structure passed.");
	if (!m_seed)
		die("Null pointer to entity structure passed.");

	if (m_seed->chromosome[m_chromosome].type != CHROMOSOME_TYPE_QUATERNION)
		dief("Chromosome %zu not a quaternion!", m_chromosome);

	if (m_seed->chromosome[m_chromosome].rnd3d.type != random_dist_none)
	{
		ran_unit_sphere_md(m_seed->chromosome[m_chromosome].q_allele[m_allele].element, 1.0, NULL, 4);
	}

	return true;
}

bool ebex_genetic_seed_quat_sample(population_t *m_pop, organism_t *m_seed, size_t m_chromosome, size_t m_allele)
{
	if (!m_seed)
		die("Null pointer to entity structure passed");

	if (m_seed->chromosome[m_chromosome].rnd.type != random_dist_none)
	{
		/// Seed by tweaking a single allele
		ebex_quat_randomize_population(&m_seed->chromosome[m_chromosome].q_allele[m_allele],
				&m_seed->chromosome[m_chromosome].rnd, 1);
	}

	return true;
}

bool ebex_genetic_seed_quat_zero(population_t *m_pop, organism_t *m_seed, size_t m_chromosome, size_t m_allele)
{
	/* Checks. */
	if (!m_pop)
		die("Null pointer to population structure passed.");
	if (!m_seed)
		die("Null pointer to entity structure passed.");


	/**
	 * Seeding.  We can't actually set a quaternion to 0 (violates a constraint), so we set
	 * it to the identity instead
	 *
	 */
	ebex_quat_set_identity(&m_seed->chromosome[m_chromosome].q_allele[m_allele]);

	return true;
}
